#include <avr/interrupt.h>
#include <avr/io.h> 

// Arduino-Based Blimp Aeroexplorer
// This code based on Blimpduino v4 by Jordi Munoz and Chris Anderson
// and is distributed under the GNU Lesser GPL

#define FORW HIGH //defining FORWARD as HIGH
#define REVERSE LOW
#define UP HIGH
#define DOWN LOW

#define led_north 12 //OK
#define led_east 17 //OK
#define led_south 11 //OK
#define led_west 13 //OK
#define led_delay 125 
#define led_cycles 1 

#define US_FRONT 8
#define US_LEFT 9
#define US_ALT_rx 15

//#define IR_FRONT 8 //define IR pins
#define IR_BACK 7 //
#define IR_RIGHT 6
//#define IR_LEFT 9 //

#define max16 2000 //max position in useconds
#define min16 1000 //min position useconds

#define MAX_motor 45 //Maximun speed of the motors
#define MIN_motor -45

#define MAX_vect 135 //Maximun pitch of the vector in navigation mode... 
#define MIN_vect 45

//Motor power adjust for navigation mode

#define forward_motor_right 40 //The amount of thrust applied to the right motor when traveling forward, you need to adjust this value if the blimp doesn't fly straight 
#define forward_motor_left 40 //Left motor, same as above..

#define turn_motors 50 // The total thrust applied to the motor when you are making turns (right or left)... Try to put a low value to avoid blimp oscillations.. 

#define PID_dt 100 //The refresh rate in milliseconds of the PID system... 
#define PID_dt_seconds PID_dt/1000 

#define low_battery_limit 6000 //6.0volts

#define read_ch2 (PINB & (1 << 6))//Port B, pin 6 as channel 1;
#define read_ch1 (PINB & (1 << 7))//Port B, pin 7 as channel 2;

#define ping_mode 1 // 0= digital mode, 1=analog mode. 

//Altitude hold dead zone variables 
//remember we only use one motor at a time. One pushes up and the other pushes down 
#define up_dead_zone 30 // We use a greater value here because the propeller is less efficient in this direction
#define down_dead_zone 20

//PID gains Kp, Ki , Kd
//Holding altitude mode only gains (no navigation).
#define alt_Kp 3
#define alt_Ki .1
#define alt_Kd 200
//Navigation mode altitude gains.
#define nav_Kp 5
#define nav_Ki 0
#define nav_Kd 0

/**************************************************************
 * 
 ***************************************************************/

/* UltraSonic stuff   */
unsigned long timer_ultrasonic=0;
unsigned int altitude=0;
float average_altitude=0;

/*Infrared stuff*/
unsigned long timer_irs=0;
unsigned int IR_cnt[4];
byte ir_compare=0;

/*Servo stuff */
unsigned long timer_servo=0;
byte servo_position=0;

/*System variables */
byte system_state=0;
float average_battery=1023;
float battery_voltage=7000;
byte last_state=5;
int startup_altitude=30;
unsigned long system_timer=0;

/*AutoPilot Stuff */
float kp=20.0;
float ki=5.9;
float kd=0.1;

float PID_I_altitude=0;
float PID_D_altitude=0;
float PID_average_altitude=0;
float PID_prev_error_altitude=0;
float PID_output_altitude=0;
float PID_error=0;
float PID_P=0;
//float PID_current_Point=0;;
unsigned long timerPID_altitude=0;

//RC mode stuff
unsigned int clock1=0;//Clock to count the pulse length
unsigned int clock2=0;
int ch1_position=0;  //This variable stores the raw position of channel 1
int ch2_position=0; //The same as above but for ch2. 
byte ch1_state_flag=0;//Something used to messure the pulse length... 
byte ch2_state_flag=0;
int rc_ch1_central; //The central position of the control
int rc_ch2_central;
int MAX_RC=20;   //Normally is 40, but is auto adjustable... 

unsigned int fore_reads[9] = {0};
unsigned int star_reads[9] = {0};
byte current_read_index = 0;
byte shortest_fore_read = 0;
byte shortest_fore_read_index = 0;
boolean inNewState = true;

float constrain_float(float value, float max, float min);

void smart_delay( unsigned int time )
{
  char serialBuffer[ 100 ];
  int serialAvailableCount = 0;
  unsigned long start_time = millis( );
  while( time == 1 || ( millis( ) - start_time ) < time )
  {    
    if( ( serialAvailableCount = Serial.available( ) ) > 1 )
    {
        for( int i = 0; i < serialAvailableCount; i++ )
        {
          serialBuffer[ i ] = Serial.read( );
        }
                
        if( serialAvailableCount > 1 && strncmp( serialBuffer, "go", 2 ) == 0 )
        {
          Serial.println( "Received a start command...starting system." );
          system_state = 1;
          inNewState = true;
          Serial.flush( );
        }
        else if( serialAvailableCount > 1 && strncmp( serialBuffer, "id", 2 ) == 0 )
        {
          Serial.println( "START 1 1 STOP" );
          Serial.flush( );
        }
        else if( serialAvailableCount > 1 && strncmp( serialBuffer, "st", 2 ) == 0 )
        {
          Serial.println( "Received a stop command...halting system." );
          stopMotors( );
          system_state = 100;
          inNewState = true;
          Serial.flush( );
        }
    }
    
    if( time == 1 )
    {
      break;
    }
  }
}

void setup()
{
  Serial.begin(19200);
  
  Serial.print("Starting System... ");
//  Init_RC();
  Init_LEDs(); 
//  Init_IRs();
  init_ultrasonic();   
  Init_motors();
  Serial.println("OK!!!");
  
  average_battery=analogRead(0);//refreshing..     
  Serial.print("Battery Analog: ");
  Serial.println((int)average_battery);
}

void loop()
{ 
  
  switch(system_state)//This is the system state switch.. 
  {
  case 0:
      // *** BOOT UP MODE *** //    
    Serial.print("Reading startup altitude... ");
    startup_altitude=analogRead(2); //Reading the startup altitude, which the system will try to maintain
    Serial.println(startup_altitude);
    delay(1000); // wait one second
    Init_servo();//Starting servo services...AKA timer 1 settings =).. 
    test_servos(1000);
    delay(400);
    system_state = 100;
    break;
  case 100:
    smart_delay( 500 );
    Battery_protection( );
    break;
  case 1:
    // *** TURN AND SCAN MODE *** //
    inNewState = turnAndScanMode( inNewState );
    system_refresh(150);
    break;
  case 2:
    // *** NAVIGATE TO THE WALL MODE *** //
    inNewState = navigateToTheWallMode( inNewState );
    system_refresh(150); //This is the system refresh loop, which runs every 250 miliseconds
    break;
  case 3:
    // *** FOLLOW THE WALL MODE *** //
    inNewState = followTheWallMode( inNewState );
    system_refresh(150); //This is the system refresh loop, which runs every 250 miliseconds
    break;
    /**************************************************************
     * System State 4: Will only enter if we are running out of battery, just to protect your "made in china" $11.99 lipo... 
     ***************************************************************/
  case 4:
    stopMotors( );
    fun_LEDs(2, 5, 100);
    Serial.print("LOW BATTERY!: "); 
    Serial.println((int)battery_voltage);
    Battery_protection();

    break;

   case 5:
    smart_delay( 1000 );
   break;  
    /**************************************************************
     * 
     ***************************************************************/
  default:
    system_state=0;
    break;
  }
  
}//END OF THE MAIN LOOP HERE!!!!!!



/**************************************************************
 * Function to process system services, like battery level and print some parameters. 
 ***************************************************************/
void system_refresh(unsigned int refresh_rate)
{
  unsigned long now = millis( );
  if( now - system_timer > refresh_rate) 
  {
    system_timer=now;
    Battery_protection(); //Function locate bellow this to check the battery power... 
    if( battery_voltage < 7200 )
    {
      Serial.print( "BATT: " );
      Serial.println( (int)battery_voltage );
    }
    smart_delay( 1 );
  }   
}
/**************************************************************
 * Function to analyse the battery level and trigger the safe mode, (to avoid killing the lipo battery)
 ***************************************************************/
void Battery_protection(void)
{

  average_battery=(((float)average_battery*.99)+(float)((float)analogRead(0)*.01));// I call it a dynamic average

  battery_voltage=(float)((float)average_battery*(float)4.887586)*(float)2;//converting values to millivolts.......


  if(battery_voltage < low_battery_limit)//If the battery voltage is less than the low_battery_limit, change the states and turn off the LED's... 
  {
    off_leds(); // This is not needed.. 
    system_state=4;

  }   
}

// the return value indicates true if we changed system_state
// that way the next state will know it's just started,
// otherwise false so we know we've been running
boolean turnAndScanMode( boolean shouldReset )
{
    static unsigned int foreReadTotal = 0;
    static unsigned int starReadTotal = 0;
    static unsigned int foreCount = 0;
    static unsigned int starCount = 0;
    static unsigned int scanModeStart = 0;
    
    unsigned int spinTime = 12500;
    unsigned int foreRead = 0;
    unsigned int starRead = 0;
    boolean readFore = false;
    boolean readStar = false;
    
    if( shouldReset )
    {
      Serial.println( "Resetting in mode 1..." );
      foreReadTotal = 0;
      starReadTotal = 0;
      foreCount = 0;
      starCount = 0;
      scanModeStart = 0;
    }
    
    if( battery_voltage > 7700 )
    {
      spinTime -= 1000;
    }
    
    // shouldReset indicates that this method has been called for the first time.
    // pass that along to the motor methods so they know to start the counter
    if( !pulseMotorTurnRight( 35, spinTime, 1000, shouldReset ) )
    {
      // the scan is done, use whatever data we have buffered up
      if( foreCount >= 5 && starCount >= 5 )
      {
        float starAverage = 0;
        float foreAverage = 0;
        foreAverage = ( float )foreReadTotal / foreCount;
        foreReadTotal = 0;
        foreCount = 0;
        starAverage = ( float )starReadTotal / starCount;
        starReadTotal = 0;
        starCount = 0;
        // send serial message with forestar reading, robot #1, msg type #2, system_state = 1
        // TODO: change this to a function call
        Serial.print( "START 1 2 " );
        Serial.print( ( unsigned int )system_state );
        Serial.print( " " );
        Serial.print( ( ( float )( millis( ) - scanModeStart - 1000 ) / ( float )( spinTime - 1000 ) ) * 360.0 );
        Serial.print( " " );
        Serial.print( foreAverage );
        Serial.print( " " );
        Serial.print( starAverage );
        Serial.println( " STOP" );
      }
      // we finished the scan, switch to the next
      // system_state
      Serial.println( "Switching to system state 2..." );
      system_state = 2;
      return true;
    }
    
    if( scanModeStart == 0 )
    {
      scanModeStart = millis( );
    }

    if( millis( ) - scanModeStart < 1000 )
    {
      return false;
    }
    
    foreRead = pulseForwardUS( spinTime/300, &readFore );
    starRead = pulseStarboardUS( spinTime/300, &readStar );

    if( readFore )
    {
      foreReadTotal += foreRead;
      foreCount++;
    }

    if( readStar )
    {
      starReadTotal += starRead;
      starCount++;
    }
    
    if( foreCount >= 10 && starCount >= 10 )
    {
      float starAverage = 0;
      float foreAverage = 0;
      foreAverage = ( float )foreReadTotal / foreCount;
      foreReadTotal = 0;
      foreCount = 0;
      starAverage = ( float )starReadTotal / starCount;
      starReadTotal = 0;
      starCount = 0;
      // send serial message with forestar reading, robot #1, msg type #2
      // TODO: change this to a function call
      Serial.print( "START 1 2 " );
      Serial.print( ( unsigned int )system_state );
      Serial.print( " " );
      Serial.print( ( ( float )( millis( ) - scanModeStart - 1000 ) / (float)(spinTime - 1000) ) * 360.0 );
      Serial.print( " " );
      Serial.print( foreAverage );
      Serial.print( " " );
      Serial.print( starAverage );
      Serial.println( " STOP" );
    }
    return false;
}

// the return value indicates true if we changed state, that way the
// next state will know if it just started, otherwise false
// so we know we've been running
boolean navigateToTheWallMode( boolean shouldReset )
{
  // when we first enter this state, we need to pulse the motors
  // back to the left to stop the clockwise motion.  once that's done,
  // we can start heading to the wall
  static boolean shouldPulseMotors = true;
  static boolean motorsAreOn = false;
  static unsigned int targetStarDistanceTotal = 0;
  static unsigned int targetStarDistanceCount = 0;
  static unsigned int currentForeDistanceCount = 0;
  static unsigned int currentForeDistanceTotal = 0;
  static unsigned int currentStarDistanceCount = 0;
  static unsigned int currentStarDistanceTotal = 0;
  unsigned int starReading = 0;
  unsigned int currentForeDistance = 0;
  unsigned int currentStarDistance = 0;
  boolean readStar = false;
  boolean readFore = false;
  float averageForeDistance = 0;
  unsigned int currentLeftSpeed = 25;
  unsigned int currentRightSpeed = 25;
  
  if( shouldReset )
  {
    shouldPulseMotors = true;
    motorsAreOn = false;
    targetStarDistanceTotal = 0;
    targetStarDistanceCount = 0;
    currentForeDistanceTotal = 0;
    currentForeDistanceCount = 0;
    currentStarDistanceTotal = 0;
    currentStarDistanceCount = 0;
  }
  
  if( shouldPulseMotors )
  {
    // pulse back to the left one good time to counteract
    // the clockwise motion.  if the motors have gone off,
    // set a flag to continue moving to the wall
    // **shouldReset is only true the first time through**
    if( !pulseMotorTurnLeft( 40, 2800, 1000, shouldReset ) )
    {
      shouldPulseMotors = false;
      smart_delay( 300 );
    }
    // take a star reading while we're here
    starReading = pulseStarboardUS( 50, &readStar );
    if( readStar )
    {
      targetStarDistanceTotal += starReading;
      targetStarDistanceCount++;
    }
    
    return false;
  }
  
  // if we're here, the motors have finished pulsing back
  // to the left.  start headin to the wall, being mindful
  // of the starboard sensor
  if( targetStarDistanceCount == 0 )
  {
    // whoops, we didn't get a star reading, take a raw reading
    targetStarDistanceTotal = raw_starboard_us( );
    targetStarDistanceCount = 1;
  }
  
  if( !motorsAreOn )
  {
    goForward( currentLeftSpeed, currentRightSpeed );
    motorsAreOn = true;
  }
  
  currentForeDistance = pulseForwardUS( 50, &readFore );
  if( readFore )
  {
    currentForeDistanceTotal += currentForeDistance;
    currentForeDistanceCount++;
  }
  
  if( currentForeDistanceCount > 5 )
  {
      averageForeDistance = ( float )currentForeDistanceTotal / currentForeDistanceCount;
      currentForeDistanceCount = 0;
      currentForeDistanceTotal = 0;
      // robot #1, msg type #3
      Serial.print( "START 1 3 " );
      Serial.print( ( unsigned int )system_state );
      Serial.print( " " );
      Serial.print( averageForeDistance );
      Serial.print( " " );
      Serial.println( " STOP" );
      
    if( averageForeDistance < 150 )
    {
      // we're closing in on the wall, switch to navigate wall mode
      stopMotors( );
      Serial.println( "Switching to system state 3..." );
      system_state = 5;
      return true;
    }
  }
  
  return false;
}

boolean followTheWallMode( boolean shouldReset )
{
  // when we first enter this state, we need to pulse the motors
  // back to the right to stop the counterclockwise drift.  once that's done,
  // we monitor the star sensor to keep us along a wall as we head to the next one
  static boolean shouldPulseMotorReverse = true;
  static boolean shouldPulseMotorRight = false;
  static boolean shouldPulseMotorLeft = false;
  static boolean resetMotors = true;
  static boolean motorsAreOn = false;
  static unsigned int targetStarDistanceTotal = 0;
  static unsigned int targetStarDistanceCount = 0;
  static unsigned int currentForeDistanceCount = 0;
  static unsigned int currentForeDistanceTotal = 0;
  static unsigned int currentStarDistanceCount = 0;
  static unsigned int currentStarDistanceTotal = 0;
  unsigned int starReading = 0;
  unsigned int currentForeDistance = 0;
  unsigned int currentStarDistance = 0;
  boolean readStar = false;
  boolean readFore = false;
  float averageForeDistance = 0;
  unsigned int currentLeftSpeed = 20;
  unsigned int currentRightSpeed = 30;
  
  if( shouldReset )
  {
    shouldPulseMotorReverse = true;
    shouldPulseMotorRight = false;
    shouldPulseMotorLeft = false;
    resetMotors = true;
    motorsAreOn = false;
    targetStarDistanceTotal = 0;
    targetStarDistanceCount = 0;
    currentForeDistanceTotal = 0;
    currentForeDistanceCount = 0;
    currentStarDistanceTotal = 0;
    currentStarDistanceCount = 0;
  }
  
  if( shouldPulseMotorReverse )
  {
    if( !pulseMotorGoBackward( 40, 40, 3000, 1000, resetMotors ) )
    {
      shouldPulseMotorReverse = false;
      shouldPulseMotorRight = true;
      resetMotors = true;
      smart_delay( 12000 );
      return false;
    }
    resetMotors = false;
    return false;
  }
  
  if( shouldPulseMotorRight )
  {
    // pulse back to the right one good time to counteract
    // the counterclockwise motion.  if the motors have gone off,
    // set a flag to continue moving to the wall
    // shouldReset is only true the first time through.
    if( !pulseMotorTurnRight( 35, 1560, 1000, resetMotors ) )
    {
      shouldPulseMotorRight = false;
      shouldPulseMotorLeft = true;
      resetMotors = true;
      smart_delay( 3000 );
      return false;
    }
    resetMotors = false;
    return false;
  }
  
  if( shouldPulseMotorLeft )
  {
    if( !pulseMotorTurnLeft( 35, 600, 1000, resetMotors ) )
    {
      shouldPulseMotorLeft = false;
      smart_delay( 3000 );
    }
    resetMotors = false;
    // take a star reading while we're here
    starReading = pulseStarboardUS( 50, &readStar );
    if( readStar )
    {
      targetStarDistanceTotal += starReading;
      targetStarDistanceCount++;
    }
    
    return false;
  }
  
  // if we're here, the motors have finished pulsing back
  // to the left.  start headin to the wall, being mindful
  // of the starboard sensor
  if( targetStarDistanceCount == 0 )
  {
    // whoops, we didn't get a star reading, take a raw reading
    targetStarDistanceTotal = raw_starboard_us( );
    targetStarDistanceCount = 1;
  }
  
  if( !motorsAreOn )
  {
    goForward( 15, 30 );
    motorsAreOn = true;
  }
  
  currentForeDistance = pulseForwardUS( 50, &readFore );
  if( readFore )
  {
    currentForeDistanceTotal += currentForeDistance;
    currentForeDistanceCount++;
  }
  
  if( currentForeDistanceCount > 5 )
  {
      averageForeDistance = ( float )currentForeDistanceTotal / currentForeDistanceCount;
      currentForeDistanceCount = 0;
      currentForeDistanceTotal = 0;
      // robot #1, msg type #3
      Serial.print( "START 1 3 " );
      Serial.print( ( unsigned int )system_state );
      Serial.print( " " );
      Serial.print( averageForeDistance );
      Serial.print( " " );
      Serial.println( " STOP" );
      
    if( averageForeDistance < 150 )
    {
      // we're closing in on the wall, switch to navigate wall mode
      stopMotors( );
      motorsAreOn = false;
      Serial.println( "Switching to system state 3..." );
      system_state = 3;
      return true;
    }
  }
  
  return false;
}
